
#include <Arduino.h>
#include <config.h>
#include "HeliOS.h"
#include <SPI.h>
#include <mcp_can.h>
#include "max6675.h"


void taskThermo_Get(xTask task_, xTaskParm parm_) ;
void taskCanSend(xTask task_, xTaskParm parm_) ;
char msgString[128];                        // Array to store serial string

MCP_CAN CAN0(CANBUS_PIN);     // Set CS to pin 5
//emperature_data_t temperature_data= {0.0,0.0};


byte CAN_FRAME_data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
byte sndStat;


int thermoDO = 12;
int thermoCLK = 13;
int thermoCS_BT = 10;
int thermoCS_ET = 9;




MAX6675 thermocoupleBT(thermoCLK, thermoCS_BT, thermoDO);
MAX6675 thermocoupleET(thermoCLK, thermoCS_ET, thermoDO);

void taskThermo_Get(xTask task_, xTaskParm parm_) 
    {

            //CAN_FRAME_data[0]=thermocoupleBT.readCelsius()*100 ;
            //CAN_FRAME_data[1]=thermocoupleET.readCelsius()*100 ;
    Serial.println(thermocoupleBT.readCelsius()*100);
    }



void taskCanSend(xTask task_, xTaskParm parm_) 
    {

digitalWrite(WORK_LED,HIGH);

    Serial.println("taskCanSend");
     sndStat = CAN0.sendMsgBuf(CAN_ID, CAN_DLC, CAN_FRAME_data);

    if(sndStat == CAN_OK){
    Serial.println("Message Sent OK!");
  } else {
    sprintf(msgString,"Error Message:%d",sndStat);
    Serial.println(msgString);
  }

  digitalWrite(WORK_LED,LOW);
    }



void setup() {

pinMode(WORK_LED,OUTPUT);
digitalWrite(WORK_LED,LOW);
Serial.begin(BAUDRATE);


Serial.println("START PGM");
//init CanBus mcp2515


digitalWrite(WORK_LED,HIGH);
  if(CAN0.begin(MCP_ANY, CAN_125KBPS, MCP_16MHZ) == CAN_OK) Serial.println("MCP2515 Initialized ");
  else Serial.println("Error Initializing");
  CAN0.setMode(MCP_NORMAL);   // Change to normal mode to allow messages to be transmitted

       
sndStat = CAN0.sendMsgBuf(CAN_ID, CAN_DLC, CAN_FRAME_data);
    if(sndStat == CAN_OK){
    Serial.println("Message Sent OK!");
  } else {
    Serial.println("Error Message...");
  }
digitalWrite(WORK_LED,LOW);

//setting HeliOS

  xSystemInit();
  xTask GetThermoTask = xTaskCreate( "THERMO", taskThermo_Get , NULL);
  xTask CANBUSSendTask = xTaskCreate("SEND", taskCanSend , NULL);
 /* Check to make sure the task was created by xTaskCreate() before
  attempting to use the task. */

 if (GetThermoTask && CANBUSSendTask ) { 
    /* Place the task in the "waiting" state so it will respond to task events. */
    /* In this case MAX6675 need some time to init , so put task in waiting mode */
 
    xTaskWait(GetThermoTask);
    xTaskWait(CANBUSSendTask);



    xTaskChangePeriod(GetThermoTask, 750); //1000ms 
    xTaskChangePeriod(CANBUSSendTask, 1000); //1000ms 

    /* Pass control to the HeliOS scheduler. The HeliOS scheduler will
    not relinquish control unless xTaskSuspendAll() is called. */

    xTaskStartScheduler();

    /* If the scheduler relinquishes control, do some clean-up by
    deleting the task. */
    xTaskDelete(GetThermoTask);
    xTaskDelete(CANBUSSendTask);
 }

  /* Halt the system. Once called, the system must be reset to
  recover. */
  xSystemHalt();

}

void loop() {
  // put your main code here, to run repeatedly:
}

